/*
 * Servo.c
 *
 *  Created on: Nov 29, 2012
 *      Author: gmkado
 */
#include "hw_types.h"
#include "pwm.h"
#include "hw_memmap.h"
#include "sysctl.h"
#include "gpio.h"
#include "Servo.h"
#include <stdlib.h>
#include <stdio.h>
#include "MagicSmokeMachine.h"


#define DELAY_PER_DEGREE 50000
#define MAX_WIDTH 2.3
#define MIN_WIDTH .4
#define PW_SLOPE (MAX_WIDTH-MIN_WIDTH)/(MAX_ANGLE-MIN_ANGLE)
#define PW_INTERCEPT (MIN_WIDTH*MAX_ANGLE - MAX_WIDTH*MIN_ANGLE)/(MAX_ANGLE-MIN_ANGLE)
#define SERVO_BASE 		GPIO_PORTF_BASE

#define SERVO_PIN 		GPIO_PIN_2
#define SERVO_PERIPH 	SYSCTL_PERIPH_GPIOF
#define PWM_GEN			PWM_GEN_2
#define PWM_OUT_BIT		PWM_OUT_4_BIT
#define PWM_OUT 		PWM_OUT_4
static angle_t currAngle;

void initServo(void)
{
   unsigned long ulPeriod;

    //
    // Set the clocking to run directly from the crystal.
    //

    SysCtlPWMClockSet(SYSCTL_PWMDIV_4);

    //
    // Enable the peripherals used by this example.
    //
    SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM);
    SysCtlPeripheralEnable(SERVO_PERIPH);

    //
    // Set GPIO H0 as PWM pin.
    //
    GPIOPinTypePWM(SERVO_BASE, SERVO_PIN);

    //
    // Compute the PWM period based on the system clock.
    //
    ulPeriod = TICKS_PER_SEC/4/ 50;

    //
    // Set the PWM period to 50 Hz.
    //
    PWMGenConfigure(PWM_BASE, PWM_GEN, PWM_GEN_MODE_UP_DOWN | PWM_GEN_MODE_NO_SYNC);
    PWMGenPeriodSet(PWM_BASE, PWM_GEN, ulPeriod);

    //
    // Enable the PWM2 output signals.
    //
    PWMOutputState(PWM_BASE, PWM_OUT_BIT, true);

    //
    // Enable the PWM generator.
    //
    PWMGenEnable(PWM_BASE, PWM_GEN);



}

void moveServo(angle_t angle)
{
	if(angle < MIN_ANGLE || angle > MAX_ANGLE)
	{
		puts("Servo:moveServo: Angle out of bounds.  Not moving servo");
	}else
	{

		float pulseWidth = PW_SLOPE*angle+PW_INTERCEPT;
		unsigned long pulseTicks = pulseWidth/1000*TICKS_PER_SEC/4;
		PWMPulseWidthSet(PWM_BASE, PWM_OUT, pulseTicks);

		//TODO: fix this to work with timer interrupts
		angle_t delta = abs(currAngle - angle);
		delayServo(DELAY_PER_DEGREE*delta);
		currAngle = angle;
	}
}

/* Move servo counterclockwise STEPANGLE degrees*/
void moveServoCCW(angle_t stepAngle)
{
	angle_t newAngle = currAngle + stepAngle;
	moveServo(newAngle);
}

/* Move servo counterclockwise STEPANGLE degrees*/
void moveServoCW(angle_t stepAngle)
{
	angle_t newAngle = currAngle - stepAngle;
	moveServo(newAngle);
}


angle_t getServoAngle(void)
{
	return currAngle;
}

void delayServo(unsigned int numTicks)
{
	unsigned int i;
	for(i = 0;  i<numTicks; i++);
}

